#include "bsp_compass.h"

SAngle stcAngle;

double compass_float;
float compass_data;

/**
 * @brief 指南针HWT101串口通讯初始化
 * @param bound 波特率115200
 */
void USART3_COMPASS_Init(void)
{
    // GPIO端口设置
    GPIO_InitTypeDef GPIO_InitStructure;
    USART_InitTypeDef USART_InitStructure;
    NVIC_InitTypeDef NVIC_InitStructure;

    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);  // 使能GPIOB时钟
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); // 使能USART3时钟

    // 串口3对应引脚复用映射
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource8, GPIO_AF_USART3); // GPIOD8复用为USART3 TX
    GPIO_PinAFConfig(GPIOD, GPIO_PinSource9, GPIO_AF_USART3); // GPIOD9复用为USART3 RX

    // USART3端口配置
    /* PD8用于TX数据输出, PD9用于RX数据输入 */
    GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_8 | GPIO_Pin_9; // PB10 复用推挽输出
    GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;            // 复用功能
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;       // 速度100MHz
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;           // 推挽复用输出
    GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;            // 上拉
    GPIO_Init(GPIOD, &GPIO_InitStructure);                   // 初始化
    // USART3 初始化设置
    USART_InitStructure.USART_BaudRate            = 115200;                         // 波特率设置
    USART_InitStructure.USART_WordLength          = USART_WordLength_8b;            // 字长为8位数据格式
    USART_InitStructure.USART_StopBits            = USART_StopBits_1;               // 一个停止位
    USART_InitStructure.USART_Parity              = USART_Parity_No;                // 无奇偶校验位
    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; // 无硬件数据流控制
    USART_InitStructure.USART_Mode                = USART_Mode_Rx | USART_Mode_Tx;  // 收发模式
    USART_Init(USART3, &USART_InitStructure);                                       // 初始化串口3

    USART_Cmd(USART3, ENABLE); // 使能串口3

    // Usart3 NVIC 配置
    USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); // 开启接收中断

    NVIC_InitStructure.NVIC_IRQChannel                   = USART3_IRQn; // 串口3中断通道
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;           // 抢占优先级1
    NVIC_InitStructure.NVIC_IRQChannelSubPriority        = 1;           // 子优先级1

    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; // IRQ通道使能
    NVIC_Init(&NVIC_InitStructure);                 // 根据指定的参数初始化NVIC寄存器
}

#if COMPASS_RECEIVE_MODE
uint8_t shujv;
uint8_t Start_Receiving;
uint8_t data[11];
uint16_t cnt_m = 0;
float JD;
float XJD;

void USART3_IRQHandler(void)
{
    if (USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) {
        CopeSerial3Data((unsigned char)USART3->DR); // 接收数据
        USART_ClearITPendingBit(USART3, USART_IT_RXNE);
        shujv = USART_ReceiveData(USART3);
        if (shujv == 0x55) // 帧头
        {
            Start_Receiving = 1;
        }
        if (Start_Receiving == 1) {
            data[cnt_m] = shujv;
            cnt_m++;
            if (cnt_m == 11) // 接受完毕，停止接收
            {
                Start_Receiving = 0;
                cnt_m           = 0;
                if (data[1] == 0x53) // 进入了0x53数据包
                {
                    XJD = (short)((data[7] << 8) | data[6]) / 32768.0 * 180;
                    JD  = (float)((data[7] << 8) | data[6]) / 32768 * 180;
                }
            }
        }
    }
    USART_ClearITPendingBit(USART3, USART_IT_ORE);
}
#else
void USART3_IRQHandler(void)
{
    // 串口3中断服务程序
    // 注意,读取USARTx->SR能避免莫名其妙的错误
    if (USART_GetITStatus(USART3, USART_IT_RXNE) != RESET) {
        CopeSerial3Data((unsigned char)USART3->DR); // 接收数据
        USART_ClearITPendingBit(USART3, USART_IT_RXNE);
    }
    USART_ClearITPendingBit(USART3, USART_IT_ORE);
}
#endif
/**
 * @brief 解包HWT101数据包,并且返回对应角度
 * @param ucData 读取DR寄存器的数据
 */
void CopeSerial3Data(unsigned char ucData)
{
    static unsigned char ucRxBuffer[250];
    static unsigned char ucRxCnt = 0;

    ucRxBuffer[ucRxCnt++] = ucData;
    if (ucRxBuffer[0] != 0x55) // 校验数据头
    {
        ucRxCnt = 0;
        return;
    }
    if (ucRxCnt < 11) { return; } // 判断一帧数据是否接收完成
    else {
        switch (ucRxBuffer[1]) {
            case 0x53:
                memcpy(&stcAngle, &ucRxBuffer[2], 8);
                break; // 判断是否为角度的数据
        }
        ucRxCnt = 0;
    }
}
/**
 * @brief HWT101 指令置零
 */
void HWT101_to_0(void)
{
    while (USART_GetFlagStatus(USART3, USART_FLAG_TC) == RESET);
    USART_SendData(USART3, 0XFF);
    while (USART_GetFlagStatus(USART3, USART_FLAG_TC) == RESET);
    USART_SendData(USART3, 0XAA);
    while (USART_GetFlagStatus(USART3, USART_FLAG_TC) == RESET);
    USART_SendData(USART3, 0X76);
    while (USART_GetFlagStatus(USART3, USART_FLAG_TC) == RESET);
    USART_SendData(USART3, 0X00);
    while (USART_GetFlagStatus(USART3, USART_FLAG_TC) == RESET);
    USART_SendData(USART3, 0X00);
}
/**
 * @brief 调用读取指南针数据,指南针断电再上电之后自动初始化校准.
 * @return compass_data 指南针数据
 */
float compass_b(void)
{
    compass_float = (double)stcAngle.Angle[2] / 32768.0 * 180;
    if ((float)compass_float < 0) {
        compass_data = 360 + (float)compass_float;
    } else {
        compass_data = (float)compass_float;
    }
    return compass_data;
}